﻿//#define VERBOSE
//#define DEBUG
using System;
using System.Diagnostics;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Runtime.Serialization;

namespace RayDen.Library.Core.Primitives
{
    using System.ComponentModel;
    using System.Globalization;
    using System.Runtime.InteropServices;

    public enum Axis : byte
    {
        PLANE_X = 0,
        PLANE_Y = 1,
        PLANE_Z = 2
    }

    [TypeConverter(typeof(ExpandableObjectConverter))]
    [Serializable]
    [StructLayout(LayoutKind.Explicit)]
    [DataContract]
    [KnownType(typeof(Vector))]
    public struct Vector : IEquatable<Vector>, IVector, ISerializableValue, IStringSerializable
    {
        public static float VecEpsilon = 0.000001f;
        public static Vector Zero = new Vector(0);
        public static Vector UnitX = new Vector(1f, 0f, 0f);
        public static Vector UnitY = new Vector(0f, 1f, 0f);
        public static Vector UnitZ = new Vector(0f, 0f, 1f);


        [DataMember]
        [FieldOffset(0)]
        public float x;
        [FieldOffset(4)]
        [DataMember]
        public float y;
        [FieldOffset(8)]
        [DataMember]
        public float z;


        public float X
        {
            get { return x; }
            set { x = value; }
        }

        public float Y
        {
            get { return y; }
            set { y = value; }
        }

        public float Z
        {
            get { return z; }
            set { z = value; }
        }
        public static readonly int SizeInBytes = 12;
        public float this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                        return x;
                    case 1:
                        return y;
                    case 2:
                        return z;
                    default:
                        throw new ArgumentException("index");
                }
            }

            set
            {
                switch (index)
                {
                    case 0:
                        x = value;
                        break;
                    case 1:
                        y = value;
                        break;
                    case 2:
                        z = value;
                        break;
                    default:
                        throw new ArgumentException("index");
                }
            }

        }

        public float Component(Axis p)
        {
            switch (p)
            {
                case Axis.PLANE_X:
                    return x;
                case Axis.PLANE_Y:
                    return y;
                case Axis.PLANE_Z:
                    return z;
                default:
                    throw new ArgumentException("index");
            }
        }


        public Vector(float a)
        {
#if DEBUG
            RayDen.Library.Core.Components.Assert.IsNotNaN(a);
#endif
            x = y = z = a;
            //     w = 0;
        }


        public Vector(float a, float b, float c)
        {
            x = a;
            y = b;
            z = c;
            // w = 0;
#if DEBUG
            RayDen.Library.Core.Components.Assert.IsNotNaN(x);
            RayDen.Library.Core.Components.Assert.IsNotNaN(y); 
            RayDen.Library.Core.Components.Assert.IsNotNaN(z);
#endif
        }

        public Vector(ref Vector v)
        {
            x = v.x;
            y = v.y;
            z = v.z;
#if DEBUG
            RayDen.Library.Core.Components.Assert.IsNotNaN(x);
            RayDen.Library.Core.Components.Assert.IsNotNaN(y);
            RayDen.Library.Core.Components.Assert.IsNotNaN(z);
#endif
            //   w = 0;
        }
        public Vector(float[] val)
        {
            x = val[0];
            y = val[1];
            z = val[2];
#if DEBUG
            RayDen.Library.Core.Components.Assert.IsNotNaN(x);
            RayDen.Library.Core.Components.Assert.IsNotNaN(y);
            RayDen.Library.Core.Components.Assert.IsNotNaN(z);
#endif
            // w = 0;
        }
        public Vector(Vector v)
        {
            x = v.x;
            y = v.y;
            z = v.z;
#if DEBUG
            RayDen.Library.Core.Components.Assert.IsNotNaN(x);
            RayDen.Library.Core.Components.Assert.IsNotNaN(y);
            RayDen.Library.Core.Components.Assert.IsNotNaN(z);
#endif
            // w = 0;
        }
        public Vector(ref Point v)
        {
            x = v.x;
            y = v.y;
            z = v.z;
#if DEBUG
            RayDen.Library.Core.Components.Assert.IsNotNaN(x);
            RayDen.Library.Core.Components.Assert.IsNotNaN(y);
            RayDen.Library.Core.Components.Assert.IsNotNaN(z);
#endif
            // w = 0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe float InvSqrt(float x)
        {
            float xhalf = 0.5f * x;
            int i = *(int*)&x;
            i = 0x5f3759ff - (i >> 1);
            x = *(float*)&i;
            x = x * (1.5f - xhalf * x * x);
            return x;
        }

        
        public float Length
        {
            get
            {
                return MathLab.Sqrt(x * x + y * y + z * z);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public float Length2()
        {
            return x * x + y * y + z * z;
        }

        public float InvLength
        {
            get
            {
                return InvSqrt(x * x + y * y + z * z);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Vector BCLNormalize()
        {
            float l = //Core(float)Math.InvSqrt(x * x + y * y + z * z);
                (1.0f / Length);
            x *= l;
            y *= l;
            z *= l;

            return this;
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Vector Normalize()
        {
            float l = 1f / (float)Math.Sqrt(x * x + y * y + z * z);
            if (float.IsInfinity(l))
            {
                return this;
            }
#if DEBUG
            RayDen.Library.Core.Components.Assert.IsNotNaN(l);
            if (float.IsInfinity(l))
            {
                return this;
            }
#endif
            //InvSqrt(x * x + y * y + z * z);
            x *= l;
            y *= l;
            z *= l;

            return this;
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void NormalizeSelf()
        {
            float l = 1f / MathLab.Sqrt(x * x + y * y + z * z);
            //InvSqrt(x * x + y * y + z * z);
            x *= l;
            y *= l;
            z *= l;

        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Clip(float vx, float min, float max)
        {
            if (vx < min)
                vx = min;
            else if (vx > max)
                vx = max;
            return vx;

        }

        public void Modify(short axis, float value)
        {
            switch (axis)
            {
                case 0:
                    x = value;
                    break;
                case 1:
                    y = value;
                    break;
                case 2:
                    z = value;
                    break;
            }
        }

        public override string ToString()
        {
            return string.Format("{0:F4},{1:F4},{2:F4}", x, y, z);
        }
        public static Vector Clip(ref Vector v, float min, float max)
        {
            if (v.x < min)
                v.x = min;
            else if (v.x > max)
                v.x = max;

            if (v.y < min)
                v.y = min;
            else if (v.y > max)
                v.y = max;

            if (v.z < min)
                v.z = min;
            else if (v.z > max)
                v.z = max;

            return v;
        }

        public override bool Equals(object obj)
        {
            if (obj is Vector)
                return this.Equals((Vector)obj);

            return false;
        }

        public bool IsZero()
        {
            return (Math.Abs(x) <= Epsilon) && (Math.Abs(y) <= Epsilon) && (Math.Abs(z) <= Epsilon);
        }


        public override int GetHashCode()
        {
            return 0;
        }

        public Vector Each(Func<float, float> action)
        {
            this.x = action(this.x);
            this.y = action(this.y);
            this.z = action(this.z);
            return this;
        }
        #region Operators

        public static float Dot(ref Vector a, ref Vector b) //TODO TO STATIC CROSS & DOT ? Operators
        {
            return (a.x * b.x + a.y * b.y + a.z * b.z);
        }

        public static float AbsDot(ref Vector a, ref Vector b) //TODO TO STATIC CROSS & DOT ? Operators
        {
            return Math.Abs(a.x * b.x + a.y * b.y + a.z * b.z);
        }

        //public static float Dot( ref Vector a, ref Point b ) //TODO TO STATIC CROSS & DOT ? Operators
        //{
        //    return (a.x * b.x + a.y * b.y + a.z * b.z);
        //}

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector Cross(ref Vector a, ref Vector b)
        {
            return
                new Vector(a.y * b.z - a.z * b.y,
                            a.z * b.x - a.x * b.z,
                            a.x * b.y - a.y * b.x);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector Cross(Vector a, Vector b)
        {
            return
                new Vector(a.y * b.z - a.z * b.y,
                            a.z * b.x - a.x * b.z,
                            a.x * b.y - a.y * b.x);
        }
        //u.y*v.z-u.z*v.y, u.z*v.x-u.x*v.z, u.x*v.y-u.y*v.x
        public static void Cross(ref Vector a, ref Vector b, out Vector r)
        {
            r.x = a.y * b.z - a.z * b.y;
            r.y = a.z * b.x - a.x * b.z;
            r.z = a.x * b.y - a.y * b.x;
        }
        public static Vector Add(ref Vector a, ref Vector b)
        {
            return new Vector(a.x + b.x, a.y + b.y, a.z + b.z);
        }

        public static Vector Sub(ref Vector a, ref Vector b)
        {
            return new Vector(a.x - b.x, a.y - b.y, a.z - b.z);
        }

        public static Vector Sub(ref Point a, ref Point b)
        {
            return new Vector(a.x - b.x, a.y - b.y, a.z - b.z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float operator &(Vector v1, Vector v2)
        {
            return Vector.Dot(ref v1, ref  v2);
        }

        //public static float operator &( Vector v1, Point v2 ) {
        //    return Vector.Dot(ref v1, ref  v2);
        //}

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector operator ^(Vector a, Vector b)
        {
            return Vector.Cross(ref a, ref b);
        }



        public static bool operator >(Vector a, Vector b)
        {
            bool result = false;
            for (int i = 0; i < 3; i++)
            {
                result = result || (a[i] > b[i]);
            }
            return result;
        }

        public static bool operator <(Vector a, Vector b)
        {
            bool result = false;
            for (int i = 0; i < 3; i++)
            {
                result = result || (a[i] < b[i]);
            }
            return result;
        }



        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector operator +(Vector v1, Vector v2)
        {
            return Vector.Add(ref v1, ref v2);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector operator -(Vector v1, Vector v2)
        {
            return Vector.Sub(ref v1, ref v2);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector operator *(Vector v1, Vector v2)
        {
            return new Vector(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector operator /(Vector v1, Vector v2)
        {
            return new Vector(v1.x / v2.x, v1.y / v2.y, v1.z / v2.z);
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector operator -(Vector v)
        {
            return new Vector(-v.x, -v.y, -v.z);
        }

        public static Vector operator +(Vector v, float f)
        {
            return new Vector(v.x + f, v.y + f, v.z + f);
        }

        public static Vector operator -(Vector v, float f)
        {
            return new Vector(v.x - f, v.y - f, v.z - f);
        }

        public static Vector operator *(Vector v, float f)
        {
            return new Vector(v.x * f, v.y * f, v.z * f);
        }

        public static Vector operator *(float f, Vector v)
        {
            return new Vector(v.x * f, v.y * f, v.z * f);
        }

        public static Vector operator /(Vector v, float f)
        {
            float divider = 1.0f / f;
            return new Vector(v.x * divider, v.y * divider, v.z * divider);
        }

        public static Vector operator /(float f, Vector v)
        {
            return new Vector(f / v.x, f / v.y, f / v.z);
        }

        public static bool operator ==(Vector a, Vector b)
        {
            return a.Equals(b);
        }

        public static bool operator !=(Vector a, Vector b)
        {
            if ((a.x != b.x) || (a.y != b.y) || (a.z != b.z))
                return true;
            return false;
        }


        //public static Vector operator *(Vector vector, Matrix matrix)
        //{
        //    return new Vector(vector.X * matrix[0, 0] + vector.Y * matrix[0, 1] + vector.Z * matrix[0, 2] + matrix[0, 3],
        //        vector.X * matrix[1, 0] + vector.Y * matrix[1, 1] + vector.Z * matrix[1, 2] + matrix[1, 3],
        //        vector.X * matrix[2, 0] + vector.Y * matrix[2, 1] + vector.Z * matrix[2, 2] + matrix[2, 3]);
        //}


        #endregion

        public float Dot(ref Vector b)
        {
            return (x * b.x + y * b.y + z * b.z);
        }

        public float Dot(ref Normal b)
        {
            return (x * b.x + y * b.y + z * b.z);
        }

        public void Cross(ref Vector a)
        {
            x = a.y * z - a.z * y;
            y = a.z * x - a.x * z;
            z = a.x * y - a.y * x;
        }

        #region Instance Operations

        public Vector Add(ref Vector a)
        {
            this.x += a.x;
            this.y += a.y;
            this.z += a.z;

            return this;
        }

        public Vector Sub(ref Vector a)
        {
            this.x -= a.x;
            this.y -= a.y;
            this.z -= a.z;

            return this;
        }

        public Vector Mul(ref Vector a)
        {
            this.x *= a.x;
            this.y *= a.y;
            this.z *= a.z;

            return this;
        }

        public Vector Div(ref Vector a)
        {
            this.x /= a.x;
            this.y /= a.y;
            this.z /= a.z;

            return this;
        }

        public Vector Add(float a)
        {
            this.x += a;
            this.y += a;
            this.z += a;

            return this;
        }

        public Vector Sub(float a)
        {
            this.x -= a;
            this.y -= a;
            this.z -= a;

            return this;
        }

        public Vector Mul(float a)
        {
            this.x *= a;
            this.y *= a;
            this.z *= a;

            return this;
        }

        public Vector Div(float a)
        {
            float d = 1 / a;

            this.x *= d;
            this.y *= d;
            this.z *= d;

            return this;
        }


        #endregion


        #region IEquatable<Vector> Members

        //UT !
        public bool Equals(Vector other)
        {
            if (((Object)other) == null)
                return false;
            float dx = (float)Math.Abs(other.x - x);
            float dy = (float)Math.Abs(other.y - y);
            float dz = (float)Math.Abs(other.z - z);

            return (dx < VecEpsilon) && (dy < VecEpsilon) && (dz < VecEpsilon);
        }

        #endregion

        #region Static Operator Realisation

        public static void Add(ref Vector a, ref Vector b, out Vector c)
        {
            c.x = a.x + b.x;
            c.y = a.y + b.y;
            c.z = a.z + b.z;
        }

        public static void Sub(ref Vector a, ref Vector b, out Vector c)
        {
            c.x = a.x - b.x;
            c.y = a.y - b.y;
            c.z = a.z - b.z;
        }

        public static void Mul(ref Vector a, ref Vector b, out Vector c)
        {
            c.x = a.x * b.x;
            c.y = a.y * b.y;
            c.z = a.z * b.z;
        }

        public static void Div(ref Vector a, ref Vector b, out Vector c)
        {
            c.x = a.x - b.x;
            c.y = a.y - b.y;
            c.z = a.z - b.z;
        }

        public static void Add(ref Vector a, float b, out Vector c)
        {
            c.x = a.x + b;
            c.y = a.y + b;
            c.z = a.z + b;
        }

        public static void Mul(ref Vector a, float b, out Vector c)
        {
            c.x = a.x * b;
            c.y = a.y * b;
            c.z = a.z * b;
        }

        public static void Sub(ref Vector a, float b, out Vector c)
        {
            c.x = a.x - b;
            c.y = a.y - b;
            c.z = a.z - b;
        }

        public static void Div(ref Vector a, float b, out Vector c)
        {
            c.x = a.x / b;
            c.y = a.y / b;
            c.z = a.z / b;
        }
        #endregion

        static readonly float Divider = 1.0f / 3.0f;
        private const float Epsilon = 1e-13f;

        public float Avg()
        {
            return (x + y + z) * Divider;
        }

        public float[] ToArray()
        {
            return new float[3] { x, y, z };
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Vector Abs()
        {
            x = (float)Math.Abs(x);
            y = (float)Math.Abs(y);
            z = (float)Math.Abs(z);

            return this;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector Normalize(Vector v)
        {
            var x = v.x;
            var y = v.y;
            var z = v.z;
            float l = 1f / MathLab.Sqrt(x * x + y * y + z * z);
            if (float.IsInfinity(l))
            {
                Console.WriteLine("Vector.Normalize divide by zero");
                //System.Diagnostics.Debugger.Break();
                return v;
            }
#if VERBOSE
            if (float.IsInfinity(l))
            {
                Debug.WriteLine("Vector.Normalize");
                //System.Diagnostics.Debugger.Break();
                return v;
            }
#endif
            
            return new Vector(x * l, y * l, z * l);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector Normalize(ref Vector v)
        {
            var x = v.x;
            var y = v.y;
            var z = v.z;
            float l = 1f / MathLab.Sqrt(x * x + y * y + z * z);
            return new Vector(x * l, y * l, z * l);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CoordinateSystem(Vector v, out Vector v2, out Vector v3)
        {
            Vector v1 = v;

            if ((float)Math.Abs(v1.x) > (float)Math.Abs(v1.y))
            {
                float invLen = 1 / MathLab.Sqrt(v1.x * v1.x + v1.z * v1.z);
                v2 = new Vector(-v1.z * invLen, 0, v1.x * invLen);
            }
            else
            {
                float invLen = 1 / MathLab.Sqrt(v1.y * v1.y + v1.z * v1.z);
                v2 = new Vector(0, v1.z * invLen, -v1.y * invLen);
            }
            v3 = (v1 ^ v2);
        }
       

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CoordinateSystem(ref Vector v, out Vector v2, out Vector v3)
        {
            Vector v1 = v;
            //v1.Normalize();

            if ((float)Math.Abs(v1.x) > (float)Math.Abs(v1.y))
            {
                float invLen = 1f / MathLab.Sqrt(v1.x * v1.x + v1.z * v1.z);
                v2 = new Vector(-v1.z * invLen, 0, v1.x * invLen);
            }
            else
            {
                float invLen = 1f / MathLab.Sqrt(v1.y * v1.y + v1.z * v1.z);
                v2 = new Vector(0, v1.z * invLen, -v1.y * invLen);
            }
            v3 = Cross(ref v1 ,ref v2);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CoordinateSystem(ref Normal v, out Vector v2, out Vector v3)
        {
            var v1 = new Vector(v.x,v.y,v.z);
            //v1.Normalize();

            if ((float)Math.Abs(v.x) > (float)Math.Abs(v.y))
            {
                float invLen = 1f / MathLab.Sqrt(v.x * v.x + v.z * v.z);
                v2 = new Vector(-v.z * invLen, 0, v.x * invLen);
            }
            else
            {
                float invLen = 1f / MathLab.Sqrt(v.y * v.y + v.z * v.z);
                v2 = new Vector(0, v.z * invLen, -v.y * invLen);
            }
            v3 = Normal.Cross(ref v , ref v2);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector SphericalDirection(float sintheta, float costheta, float phi)
        {
            return new Vector(sintheta * (float)Math.Cos(phi), sintheta * (float)Math.Sin(phi), costheta);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector SphericalDirection(float sintheta, float costheta, float phi,
                 ref Vector x, ref Vector y, ref  Vector z)
        {
            return sintheta * (float)Math.Cos(phi) * x + sintheta * (float)Math.Sin(phi) * y +
                    costheta * z;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float SphericalTheta(ref Vector v)
        {
            return (float)Math.Acos(Clip(v.z, -1, 1));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float SphericalPhi(ref Vector v)
        {
            float p = (float)Math.Atan2(v.y, v.x);
            return (p < 0) ? p + 2f * (float)Math.PI : p;
        }

        public static float Dot(ref Normal n1, ref Vector v2)
        {
            return n1.x * v2.x + n1.y * v2.y + n1.z * v2.z;
        }
        public static float Dot(ref Vector v2, ref Normal n1)
        {
            return n1.x * v2.x + n1.y * v2.y + n1.z * v2.z;
        
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float AbsDot(ref Normal a, ref Vector b)
        {
            return Math.Abs(a.x * b.x + a.y * b.y + a.z * b.z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float AbsDot(ref Vector a, ref  Normal b)
        {
            return Math.Abs(a.x * b.x + a.y * b.y + a.z * b.z);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float AbsDot(ref Normal a, Normal b)
        {
            return Math.Abs(a.x * b.x + a.y * b.y + a.z * b.z);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float AbsDot(ref Normal a, Vector b)
        {
            return Math.Abs(a.x * b.x + a.y * b.y + a.z * b.z);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float AbsDot(Vector a, ref  Normal b)
        {
            return Math.Abs(a.x * b.x + a.y * b.y + a.z * b.z);
        }

        public static Vector FromString(string v)
        {
            var dta = v.Trim().Replace(",", " ").Split(' ');
            if (dta.Length != 3)
            {
                throw new ArgumentException("string is in incorrect format {0}", v);
            }
            return new Vector(
                    float.Parse(dta[0], CultureInfo.InvariantCulture),
                    float.Parse(dta[1], CultureInfo.InvariantCulture),
                    float.Parse(dta[2], CultureInfo.InvariantCulture)
                    );
        }

        public void SetComponent(byte axis, float value)
        {
            switch (axis)
            {
                case 0:
                    x = value;
                    break;
                case 1:
                    y = value;
                    break;
                case 2:
                    z = value;
                    break;
                default:
                    throw new ArgumentException("index");
            }
        }

        public byte[] Serialize()
        {
            return new[]
                {
                    BitConverter.GetBytes(x),
                    BitConverter.GetBytes(y),
                    BitConverter.GetBytes(z)
                }.SelectMany(t => t).ToArray();
        }

        public void Deserialize(byte[] data, int offset = 0)
        {
            x = BitConverter.ToSingle(data, 0 + offset);
            y = BitConverter.ToSingle(data, 4 + offset);
            z = BitConverter.ToSingle(data, 8 + offset);
        }

        public string SerializeToString()
        {
            return this.ToString();
        }

        void IStringSerializable.FromString(string data)
        {
            this = FromString(data);
        }
    }

}